function epsilon=computeEpsilon(tau, psi, gamma, lastX, lastU)
epsilon = tau - psi * lastX - gamma * lastU;
end